/*
位姿解算类：因为Adam战队第一版代码，所以不能太复杂
数学知识：线性代数，刚体力学，摄像机的针孔模型
类名：Solve_Posetion
需要识别的代码预留一个返回装甲板矩形信息的接口;
对特定目标解算。选定目标矩形解算。
内参矩阵可能不对。
*/
#ifndef postion_H
#define postion_H
#include "opencv2/opencv.hpp"
using namespace std;
using namespace cv;
class Solve_Posetion{
    public:
        Solve_Posetion();//构造函数 初始化问题修改在这里修改
        void Set_3D_Point(int targetClass);//选择要打击的目标
        void SolvePnP();//需要把识别的目标的矩阵发送过来
        void Set_2D_Point(Point2f *tar);
        const cv::Vec2f getAngle(int Classer);//得到解算的角度（）
        double getDistance();//得到解算的距离（）
        void Debug();//测试该类的代码
    private:
        //star 以下为PnP解算的结果
    	cv::Mat _RVec = cv::Mat::zeros(3, 1, CV_64FC1);//旋转向量
	    cv::Mat _TVec = cv::Mat::zeros(3, 1, CV_64FC1);//平移向量
        double Distance;
        //end
        //star  以下为PnP解算需要的参数
        cv::Mat _Cam_Instant_Matrix;//内参矩阵
        cv::Mat _Cam_Aberration_Martix;//畸变矩阵
        //end
        double _YErr,_XErr;//解算的yaw轴与pitch的数值
        std::vector<cv::Point3f> POINT_3;//物体的世界坐标可以直接规定
        std::vector<cv::Point2f> POIN_2;
};
#endif
